/*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*\
|          Copyright (C) 2013 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |
                                                                                
     This program is free software: you can redistribute it and/or modify       
     it under the terms of the GNU General Public License version 2 as          
     published by the Free Software Foundation.                                 
                                                                                
     This program is distributed in the hope that it will be useful,            
     but WITHOUT ANY WARRANTY; without even the implied warranty of             
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              
     GNU General Public License for more details.                               
                                                                                
     You should have received a copy of the GNU General Public License          
     along with this program.  If not, see <http://www.gnu.org/licenses/>.      
                                                                                
            Jack Elston                                                         
|           elstonj@colorado.edu                                               |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/
#include "can_simulink.h"

ShmStruct shm;
bool running;

String port="50000";
String host="128.138.75.190";

Interface interface("CAN USB GUI");
CANUSB usbcan("CAN USB Interface");
UDPSimulink udpsimulink("UDP Simulink Interface");

#define RUN_RATE 1.0

int main(int argc, char *argv[])
{
	running = true;

	/*<---Initialize Signals-->*/
	signal( SIGINT, programExit);
	signal( SIGQUIT, programExit);

  sigset_t waitSigs;

  sigemptyset(&waitSigs);
  sigaddset(&waitSigs, TSIGQUIT);
  sigaddset(&waitSigs, TSIGTIMER);
  sigaddset(&waitSigs, SIGRTMIN);
  for(int j=SIGRTMIN; j <= SIGRTMAX; j++) sigaddset(&waitSigs, j);
  sigprocmask(SIG_BLOCK,&waitSigs,NULL);

	extern int optind;
	int c;

	while ((c=getopt(argc,argv,"i:p:h")) != EOF) switch( (char)c)
	{
		case 'i':
			host = String(optarg);
			break;
		case 'p':
			port = String(optarg);
			break;
		default:
			programUsage(argv[0]);
			exit(EXIT_SUCCESS);
	}

	if( optind < argc) {
		programUsage(argv[0]);
		exit(EXIT_SUCCESS);
	}

	shm.udpsocket_status.portStr = port;
	shm.udpsocket_status.hostStr = host;

	pthread_mutex_init(&shm.mtx, NULL);

	bool status;
	status = interface.init(&shm);
	if ( status == false) {
		interface.shutdown();
	} else interface.start(10, 0);

	status = usbcan.init(&shm);
	if ( status == false) {
		usbcan.shutdown();
	} else usbcan.start(500, 0);

	status = udpsimulink.init(&shm);
	if ( status == false) {
		udpsimulink.shutdown();
	} else udpsimulink.start(500, 0);

	while( running ) {
		usleep( (long int)(HZ2MILLI(RUN_RATE)*1000) );
	}

	if(interface.isRunning()) {
		interface.shutdown();
		pthread_join(interface.getTid(),NULL);
	}
	if(usbcan.isRunning()) {
		usbcan.shutdown();
		pthread_join (usbcan.getTid(),NULL);
	}
	if(udpsimulink.isRunning()) {
		udpsimulink.shutdown();
		pthread_join (udpsimulink.getTid(),NULL);
	}

	pthread_mutex_destroy(&shm.mtx);
	return 0;
}

void initSharedMemory() {
}

void programUsage(char * name)
{
	cout << "usage: " << name << " [ ARGS ]" << endl;
	cout << "\t where ARGS are: " << endl;
	cout << "\t -h        : display this help" << endl;
	cout << "\t -i IP     : UA dynamics simulator host IP" << endl;
	cout << "\t -p PORT   : UA dynamics simulator host port" << endl;
}

void programExit(int signal) {
	running = false;
}
